import java.lang.Math;
import lejos.nxt.*;
import lejos.robotics.subsumption.Behavior;
import lejos.robotics.subsumption.Arbitrator;
import lejos.robotics.navigation.DifferentialPilot;

public class LineBot {
  private DifferentialPilot pilot;
  private LightSensor lineSensor;
  private int offLine, steeringRate;
  private double lineThreshold;
  
  public LineBot() {
    /* setup the hardware */
    pilot = new DifferentialPilot(5.6f, 11.f, Motor.A, Motor.B);
    pilot.setTravelSpeed(Setup.readSetting("TravelSpeed", 4, 1));
    steeringRate = Setup.readSetting("Steering Rate", 120, 10);
    lineSensor = new LightSensor(SensorPort.S2);
    lineSensor.setFloodlight(true);
    lineThreshold = Setup.readSetting("Line Threshold (%)", 10, 5)/100.;
    offLine = lineSensor.getNormalizedLightValue();
    
    /* start behavior */
    Behavior [] behaviors = { new OffLine(this), new OnLine(this) };
    Arbitrator arbitrator = new Arbitrator(behaviors);
    arbitrator.start();
  }
  
  public boolean isOnLine() {
    System.out.println(lineSensor.getNormalizedLightValue() + "<?" + offLine);
    return Math.abs(offLine-lineSensor.getNormalizedLightValue())>lineThreshold*offLine;
  }
  
  public void spiralLeft() {
    pilot.steer(-steeringRate);
  }
  
  public void spiralRight() {
    pilot.steer(steeringRate);
  }
  
  public void stop() {
    pilot.stop();
  }
  
  public static void main (String args[]) {
    LineBot me = new LineBot();
  }
}